#include "RoadMap.h"
#include "gl/glut.h"

using namespace std;

const int KNN_K = 50;
const int MAX_EDGES = 5;

RoadMap::RoadMap(void)
{
}


RoadMap::~RoadMap(void)
{
}

void RoadMap::init(float minX, float maxX, float minY, float maxY)
{
	nn_.init(minX, maxX, minY, maxY);
}

void RoadMap::addVertex(const Vector2& vertex, const Environment2D& environment, float radius, clock_t& knnTime, clock_t& linkTime)
{
	int index = vertices_.size();

	vertices_.push_back(vertex);
	
	vector<int> neighborIndices;
	clock_t startTime = clock();
	nn_.kNearestNeighbors(vertex, neighborIndices, KNN_K);
	knnTime += clock() - startTime;

	int added = 0;
	for (unsigned int i = 0; i < neighborIndices.size(); ++i)
	{
		startTime = clock();
		bool isLinkSuccess = environment.link(vertex, vertices_[neighborIndices[i]], radius);
		linkTime += clock() - startTime;
		if (isLinkSuccess)
		{
			edges_.push_back(make_pair(neighborIndices[i], index));
			++added;
		}
		if (added == MAX_EDGES)
			break;
	}

	nn_.addVertex(vertex, index);
}

void RoadMap::render()
{
	glBegin(GL_LINES);                      
	glColor3f(0.0f, 0.0f, 1.0f);  	
	for (unsigned int i = 0; i < edges_.size(); ++i)
	{
		int index1 = edges_[i].first;
		int index2 = edges_[i].second;
		glVertex3f( vertices_[index1]._x, vertices_[index1]._y, 0.0);
		glVertex3f( vertices_[index2]._x, vertices_[index2]._y, 0.0);
	}
	glEnd();  

	glBegin(GL_POINTS);                      
	glColor3f(0.0f, 1.0f, 0.0f);  	
	for (unsigned int i = 0; i < vertices_.size(); ++i)
		glVertex3f( vertices_[i]._x, vertices_[i]._y, 0.0);
	glEnd();  
}